#ifndef __MOTOR_H
#define __MOTOR_H

#include "pid.h"

#define MOTOR_ENABLE P67
#define MOTOR_P1 PWM2P_P62
#define MOTOR_P2 PWM1P_P60
#define MOTOR_P3 PWM3P_P64
#define MOTOR_P4 PWM4P_P66
#define MOTOR_MAX  9600

#define Limit(value,max)     if(value>max)value=max;else if(value<-max)value=-max

extern int16 pwm_out_left, pwm_out_right;
extern PID_Struct PD_Angle,PI_Speed;
void motor_angle_pd(float p ,float d);
void motor_speed_pi(float p ,float i);

void motor_enable();
void motor_disable();
void motor_init();
void motor_pwm_out(int16 speed_pwmL, int16 speed_pwmR);
void motor_exe();

extern float target_angle;

#endif /*__MOTOR_H */